package clubhouse.control;

import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.OutputStreamWriter;
import java.text.DecimalFormat;
import java.util.ListIterator;
import java.util.Vector;

import clubhouse.antislam.AntiSLAM;
import clubhouse.base.Base;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class DataChecker {
	/***********Displaying Constants**********/
	/**Display of the initial robot position.**/
	public static final int initialPosition = 1;
	/**Display of the received information from node.**/
	public static final int infoReceive = 2;
	/**Display of the data error Process.**/
	public static final int error = 3;
	/**Display of the estimate robot position.**/
	public static final int robotEstimate = 4;
	/**Display of the landmark coordinates after data processing.**/
	public static final int landmark = 5;
	/**Display of the next destination.**/
	public static final int destination = 6;
	/**Display of the move information in units.**/
	public static final int moveInfo = 7;
	/**Display of the move information in pulses.***/
	public static final int movePulses = 8;
	/**Display of the new robot position.**/
	public static final int newPosition = 9;
	/**Display of the info when the simulation is stopped, finished or when the robot is detected.**/
	public static final int stopInfo = 10;
	/**Display of the Anti-SLAM process information.**/
	public static final int antiSlam = 11;
	/**Display of the EKF process information.**/
	public static final int ekf = 12;
	/**Display of the metric M1 (the minimal distance to each node during the whole simulation).**/
	public static final int displayM1 = 13;
	/**Display of the metric M2 (the average distance to closest node during the whole simulation).**/
	public static final int displayM2 = 14;
	/**Display of the metric M3 (number of step before reaching the final destination, -1 if not reach yet).**/
	public static final int displayM3 = 15;
	/**Display of the metric M4 (number of step before entering in an node acting Range, -1 if not occurred yet).*/
	public static final int displayM4 = 16;
	
	/**The size of the node communication range in units.**/
	public static final int communicationRange = 30;
	/**The size of the node sensing range in units.**/
	public static final int sensingRange = 15;
	/**The size of the node acting range in units.**/
	public static final int actingRange = 5;
	
	/**The current error attenuation factor as specified in the corresponding field in GUI.**/
	public int errorFactor;
	/**The source of current generated network, used only in scenario 3, during metrics saving.*/
	private int networkSrc;
	/**The name of the file to read a network.*/
	private String fileName = null;

	/**The current step.*/
	private int step;
	/**The current robot position [X Coordinate, Y coordinate].*/
	private double[] currentRobotPosition;
	/**Used only in scenario 1, current root mean square error.*/
	private double rootMeanSquareError;
	/**Set of all currently known landmarks, and corresponding metric M1 
	 * (the minimal distance to each node during the whole simulation) [nodeid, x, y, M1 node id].*/
	private Vector<double[]> landmarks;
	/**The current scenario*/
	private int scenario;
	/**Not used in scenario 1, the current euclidean distance to the closest node, used in M1 calculation.*/
	private double C=-1;
	/**Not used in scenario 1, the current value of metric M2, the average distance to closest node during the 
	 * whole simulation.*/
	private double M2=0;
	/**Not used in scenario 1, the current value of metric M3, number of step before reaching the final 
	 * destination, -1 if not reach yet.*/
	private int M3=-1;
	/**Not used in scenario 1, the current value of metric M4, number of step before entering in an node 
	 * acting Range, -1 if not occurred yet.*/
	private int [] M4;
	/**DecimalFormat object, used to manage the format of double display.*/
	private DecimalFormat df;
	/**The set of real landmarks (as generated).*/
	private Vector<double[]> realLandmarks;
	/**The state of simulation, true if it's a 4-stage simulation, false otherwise.*/
	private boolean completeSimulation;
	/**The state of simulation, true if it's a comparing Anti-SLAM strategy simulation, false otherwise.*/
	private boolean comparingAntiSlam;
	/**The enabling error state, true if error process is enabled, false otherwise.*/
	private boolean enableError;
	/**The enabling EKF state, true if EKF process is enabled, false otherwise.*/
	private boolean enableEkf;
	/**The enabling Anti-SLAM state, true if Anti-SLAM process is enabled, false otherwise.*/
	private boolean enableAntiSlam;
	/**The value of current Anti-SLAM strategy, equals to one of the Anti-SLAM strategy constants.*/
	private int antiSlamType;
	/**Current total number of real nodes.*/
	private int numberOfNode;
	/***/
	private int simulationNumber;
	

	/**
	 * Constructs the DataChecker object.
	 */
	public DataChecker() {
		this.clear();
		this.M4 = new int[]{-1,0};
		this.df = new DecimalFormat("#####.##"); 
	}	
	
	/**
	 * Reinitialize data checker and set simulation parameters:
	 * - error factor to the specified value in the corresponding field in GUI.
	 * - source of the network generation.
	 * @param scenario the current scenario.
	 * @param networkSrc the way that the network has been generated (from file, from panel or randomly),useful for saving metrics(only for the scenario 3).
	 * @param errorFactor the error factor specified in the corresponding field in GUI.
	 */
	public void initChecker(int scenario, int networkSrc, String fileName, int errorFactor, boolean completeSimulation ,boolean comparingAntiSlam, boolean enableError, boolean enableEkf, boolean enableAntiSlam, int antiSlamType) {
		this.landmarks = new Vector<double[]>(5,1);
		this.scenario = scenario;
		if (this.scenario==3) this.networkSrc = networkSrc;
		this.errorFactor = errorFactor;
		this.enableError = enableError;
		this.enableEkf = enableEkf;
		this.enableAntiSlam = enableAntiSlam;
		this.antiSlamType = antiSlamType;
		this.fileName = fileName;
		this.completeSimulation = completeSimulation;
		this.comparingAntiSlam = comparingAntiSlam;
		this.simulationNumber = 0;
	}
	
	/**
	 * Get the landmark with the specified id.
	 * @param nodeid the id of requested node.
	 * @return the landmark with the specified id if known, null otherwise.
	 */
	public double[] getLandmark(int nodeid) {
		double[] temp = null, result = null;
        ListIterator<double[]> li = this.landmarks.listIterator();
        while(li.hasNext()){
        	temp = li.next();
        	if (temp[0] == nodeid) {
        		result = temp;
        		break;
        	}
        }
		return result;
	}
	
	
	/**
	 * Test if the robot is in an acting Range of at least one node.
	 * @return true if the M4 metric is not at its initial value, false otherwise.
	 */
	public boolean isInActingRange(){
		return (this.M4[0]!=-1);
	}

	
	/**
	 * Get the corresponding vector index of the landmark with the specified id.
	 * @param nodeid id of the wanted node.
	 * @return the index value in the landmarks vector of the node which has the specified id if existing, -1 otherwise.
	 */
	public int getLandmarkIndex(int nodeid) {
		double[] temp = null;
		int result = -1;
		
        for (int i=0;i<this.landmarks.size();i++){
        	temp = this.landmarks.get(i);
        	if ((temp != null)&&(temp[0] == nodeid)) {
        		result = i;
        		break;
        	}
        }
		return result;
	}
	/**
	 * Get the corresponding vector index of the landmark with the specified id.
	 * @param nodeid id of the wanted node.
	 * @return the index value in the landmarks vector of the node which has the specified id if existing, -1 otherwise.
	 */
	public double[] getRealLandmarkById(int nodeid) {
		double[] temp = null, result = null;

		ListIterator<double[]> li = this.realLandmarks.listIterator();
		while (li.hasNext()) {
        	temp = li.next();
        	if ((temp != null)&&(temp[0] == nodeid)) {
        		result = temp;
        		break;
        	}
        }
		return result;
	}
	
	
	/**
	 * Add a character "s" if the specified data is more than one. Useful for display.
	 * @param data the value to check.
	 * @return the character "s" if data>1, the null character "" otherwise.
	 */
	private String isPlurial(double data){	
		if (data>1){ 
			return "s";
		} else {
			return "";
		}
	}
	
	/**
	 * Initially verify then display, now only display, information in a correct format according to the value
	 * of display type.
	 * <br/>Calculate also all simulation metrics.
	 * @param type the type of information to display, equals to one of the displaying constants.
	 * @param dataPreview all data to display according to the type.
	 * @return the string to display.
	 */
	public String verifyData(int type,double... dataPreview){
		String result = "";
		double[] data=null;
		if (dataPreview !=null){
			data = new double[dataPreview.length];
			System.arraycopy(dataPreview, 0, data, 0, dataPreview.length);
		}
		double dist;
		
		switch (type){
			case DataChecker.initialPosition:
				result = "[Initial Position]";
				result = result+"\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				result = result+"\tr: "+df.format(data[2]);
				this.currentRobotPosition[0] = data[0];
				this.currentRobotPosition[1] = data[1];
				break;
				
			case DataChecker.infoReceive:
				result = "[Data Received]";
				result = result+"\tnode "+df.format(data[0]);
				result = result+"\tx: "+df.format(data[1]);
				result = result+"\ty: "+df.format(data[2]);
				break;
				
			case DataChecker.antiSlam:
				result = "[antiSlamed node "+df.format(data[0]);
				result = result+"]\t("+df.format(data[1]);
				result = result+";"+df.format(data[2]);
				result = result+")\tbecome\t("+df.format(data[3]);
				result = result+";"+df.format(data[4])+")";
				if (!this.enableAntiSlam) {
					this.enableAntiSlam = true;
					this.antiSlamType = (int) data[5];
				}
				break;
				
			case DataChecker.error:
				result = "[Error node "+df.format(data[0]);
				result = result+"]\t("+df.format(data[1]);
				result = result+";"+df.format(data[2]);
				result = result+")\tbecome\t("+df.format(data[3]);
				result = result+";"+df.format(data[4])+")";
				if (!this.enableError) this.enableError = true;
				
				break;
				
			case DataChecker.robotEstimate:
				result = "[Robot Estimate]";
				result = result+"\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				result = result+"\tr: "+df.format(data[2]);
				break;
				
			case DataChecker.landmark:
				double[] tempLandmark = null;
				
				result = "[Landmark "+df.format(data[0])+" on "+df.format(data[1])+"]";
				result = result+"\tx: "+df.format(data[2]);
				result = result+"\ty: "+df.format(data[3]);
				
				if (this.scenario!=1){
					int tempIndex = this.getLandmarkIndex((int)data[0]);
					if (tempIndex==-1){
						if (getRealLandmarkById((int)data[0])!=null){
							this.landmarks.add(new double[]{(int)data[0],this.getRealLandmarkById((int)data[0])[1],this.getRealLandmarkById((int)data[0])[2],Base.euclideanDist(this.currentRobotPosition[0], this.currentRobotPosition[1], this.getRealLandmarkById((int)data[0])[1], this.getRealLandmarkById((int)data[0])[2])});
						}
					} else {
						tempLandmark = this.landmarks.get(tempIndex);
						dist = Base.euclideanDist(this.currentRobotPosition[0], this.currentRobotPosition[1], tempLandmark[1], tempLandmark[2]);
						if ((dist<DataChecker.actingRange)&&(this.M4[0]==-1)) {
							this.M4[0] = this.step;
							this.M4[1] = (int) data[0];
						}
						if (dist>tempLandmark[3]) dist = tempLandmark[3];
						
						if (this.C==-1) C = dist;
						else if (dist < this.C) C = dist;
						
						this.landmarks.remove(tempIndex);
						this.landmarks.add(new double[]{(int)data[0],tempLandmark[1],tempLandmark[2],dist});
					}
				}
				break;
				
			case DataChecker.destination:
				result = "[Destination]\t\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				break;
				
			case DataChecker.moveInfo:
				result = "[Move's Info]\t\td: "+df.format(data[0]);
				result = result+" unit"+this.isPlurial(data[0])+" \tr: "+df.format(data[1]);

				if (data[1]==0)	result = result+" rad";
				else { 
					if (data[2]==0){
						result = result+" rad in non-trigonometric direction";
					} else {
						result = result+" rad in trigonometric direction";
					}
				}
				break;
				
			case DataChecker.movePulses:
				result = "[Move's Pulses]\td: "+df.format(data[0]);
				result = result+" pulse"+this.isPlurial(data[0])+" \tr: "+df.format(data[1]);
				if (data[1]==0)	result = result+" pulse";
				else { 
					if (data[2]==0){
						result = result+" pulse"+this.isPlurial(data[1])+" in non-trigonometric direction";
					} else {
						result = result+" pulse"+this.isPlurial(data[1])+" in trigonometric direction";
					}
				}		
				break;
				
			case DataChecker.newPosition:
				result = "[New Position]\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				result = result+"\tr: "+df.format(data[2])+"\n";
				
				if (this.scenario != 1){
					if (( Base.euclideanDist(data[0], data[1], Base.FINALDESTINATION[0],Base.FINALDESTINATION[1]) <= DataChecker.actingRange)&&(this.M3==-1)) this.M3 = this.step;
				}
				
				this.step++;
				this.M2 = this.M2 + this.C;
				this.C = -1;

				break;
			case DataChecker.stopInfo:
				if (this.scenario == 1){
					result="\n\n|**************Experiment's Informations*****************|\n";
					result=result+"Root Mean Square Error = "+df.format(this.rootMeanSquareError)+"\n";
					result=result+"|************************************************************|\n\n \n";
				} else {
					result="\n\n|**************Experiment's Informations*****************|\n";
					result=result+"Experiment time = "+this.step+" step"+this.isPlurial(this.step)+"\n";
					if (this.M3 != -1){
						result=result+"Congratulations! The robot has reached its destination point ("+Base.FINALDESTINATION[0]+","+Base.FINALDESTINATION[1]+") at the step "+this.M3+"\n";
					} else {
						result=result+"Too bad! The robot hasn't (still?) reached its destination point ("+Base.FINALDESTINATION[0]+","+Base.FINALDESTINATION[1]+")\n";
					}
					double[] temp;
					for (int i=0;i<this.landmarks.size();i++){
						temp = this.landmarks.get(i);
						result=result+"Minimal distance to landmark "+df.format(temp[0])+" is "+df.format(temp[3])+" unit"+this.isPlurial(temp[3])+"\n";
					}
					if (this.M4[0] == -1){
						result = result + "The robot has never been sensed. Congratulations!\n";
					} else {
						result = result + "The robot has been sensed for the first time at the step "+this.M4[0]+" by the node "+this.M4[1]+"\n";
					}
					result=result+"|************************************************************|\n \n";	
				}
				
				
				break;
			case DataChecker.ekf:
				result="[EKF]\tX= "+df.format(data[0])+"\tY= "+df.format(data[1]);
				if (!this.enableEkf) this.enableEkf = true;
				break;
			case DataChecker.displayM1:
				for (int i=0;i<this.numberOfNode;i++){
					tempLandmark = this.getLandmark(i);
					if ( tempLandmark!=null ){
						result = result + "M1,"+df.format(tempLandmark[0])+" = "+df.format(tempLandmark[3])+" unit"+this.isPlurial(this.M2/this.step)+"\n";
					}
				}	 
				break;
			case DataChecker.displayM2:
				if (this.step!=0){
					result = "M2("+this.step+") = "+df.format(this.M2/this.step)+" unit"+this.isPlurial(this.M2/this.step);
				}
				break;
			case DataChecker.displayM3:
				if (this.M3 != -1){
					result = "M3 = "+this.M3+"";
				} else {
					result = "M3 = -1";
				}
				break;
			case DataChecker.displayM4:
				if (this.M4[0] == -1){
					result = "M4 = -1";
				} else {
					result = "M4 = "+this.M4[0]+" by the node "+this.M4[1]+"";
				}
				break;
			default:
				break;
		}
		return result+"\n";
	}
	
	/**
	 * Get the Anti-SLAM strategy in a string from the Anti-SLAM strategy constant, used for display.
	 * @return the string containing the correct Anti-SLAM strategy.
	 */
	private String antiSlamToString(){
		switch (this.antiSlamType){
			case AntiSLAM.TRANSLATION:
				return "Translation_AntiSLAM";
			case AntiSLAM.ROTATION:
				return "Rotation_AntiSLAM";
			case AntiSLAM.HYBRID:
				return "Hybrid_AntiSLAM";
			default:
				return "";
		}
	}
	
	/**
	 * Set the network source file name to the specified one.
	 * @param fileName the new file name to set.
	 */
	public void setFileName(String fileName){
		this.fileName = fileName;
	}
	
	/**
	 * Save all metrics to a text file named according the current scenario, the source of generated landmarks 
	 * (if scenario 3), the number of these landmarks and the error attenuation factor.
	 * <br/>Filename example: "metrics_sc3(Random)_nb10_err10.txt", for a simulation of scenario 3 of 
	 * 10 landmarks generated randomly and an attenuation factor equals to 10.
	 */
	public void saveInfo(){
		FileOutputStream fout;
		OutputStreamWriter out;
		File fileOut;
		String name = "";
		/**Get the file name according to simulation parameters**/
		if (this.comparingAntiSlam) name += "AS_Comparison_";
		name += "metrics_sc"+this.scenario;
		if (this.enableError) name = name + "_err"+this.errorFactor;
		if (this.enableEkf) name = name + "_ekf";
		if (this.enableAntiSlam) name = name + "_"+this.antiSlamToString();
		/********************************************************/
		
		/**Get the folder name according to the network**/
		String folderName = "";
		switch (this.scenario){
			case 1:
				folderName = "default_scenario_1\\";
				break;
			case 2:
				folderName = "default_scenario_2\\";
				break;
			case 3:
				switch (this.networkSrc){
					case Base.RANDOM:
						folderName = "random "+this.numberOfNode+"node"+this.isPlurial(this.numberOfNode)+ " network\\";
						break;
					case Base.READFROMFILE:
						folderName = this.fileName+"\\";
						break;
					case Base.READFROMPANEL:
						folderName = this.fileName+"\\";
						break;
					case Base.REALNETWORK:
						folderName = "real network\\";
						break;
					default:
						break;
				}
				break;
			default:
				break;
		}
		if (this.completeSimulation) folderName += "Complete Simulation\\";
		/************************************************/
		try
		{
			String data = null;
			double [] tempLandmark= null;
		    //Create file in folder Results if doesn't exist
			fileOut = new File("results\\"+folderName+name+".txt");
			File temp = fileOut.getParentFile();
			if (!temp.exists()) temp.mkdir();
			if (!fileOut.exists()) fileOut.createNewFile();
			
			// Open an output stream
		    fout = new FileOutputStream (fileOut,true);
		    out = new OutputStreamWriter(fout, "UTF-8");
		    
		    data = "";
		    data += "(" + this.simulationNumber + ")\t";
		    for (int i=1;i<=this.numberOfNode;i++){
		    	tempLandmark = this.getLandmark(i);
		    	if (tempLandmark == null){
			    	data = data + "-1\t";
		    	} else {
			    	data = data + df.format(tempLandmark[3])+ "\t";
		    	}
		    }

		    data = data + df.format(this.M2/this.step)+"\t"+df.format(this.M3)+"\t"+df.format(this.M4[0])+"\t"+df.format(this.M4[1])+"\n";
			
			out.append(data);

		    // Close our output stream
		    out.close();		
		}
		catch (IOException e){
			System.err.println ("Unable to write to file");
			System.exit(-1);
		}
		
	}

	/**
	 * Reinitialize all parameters, variables and metrics.
	 */
	public void clear() {
		this.step = 0;
		this.currentRobotPosition = new double [2];
		this.rootMeanSquareError = 0;
		this.M2 = 0;
		this.M3 = -1;
		this.M4 = new int[]{-1,0};
		this.landmarks = new Vector<double[]>(5,1);
		this.enableAntiSlam = false;
		this.enableEkf = false;
		this.enableError = false;
		this.antiSlamType = -1;
	}

	/**
	 * Update real landmarks from specified landmarks.
	 * @param realLandmarks the new real landmarks as generated or as received before processing them.
	 */
	public void setRealLandmarks(Vector<double[]> realLandmarks) {
		this.realLandmarks = realLandmarks;
		this.numberOfNode = this.realLandmarks.size();
	}
	
	
	/**
	 * @param number
	 */
	public void setSimulationNumber(int number){
		this.simulationNumber = number;
	}
}
